#include "ros_utility.hpp"
#include <ros/package.h>
#include <rosbag/bag.h>
#include <sensor_msgs/Imu.h>

const std::string packageName = "adapter";

class PreprocessorMetacam
{
public:
    ros::Subscriber sub_imu;
    ros::Subscriber sub_lidar;
    std::vector<CameraBagSaver> cameraBagSavers;
    std::shared_ptr<rosbag::Bag> bag;
    std::string topicImuIn;
    std::string topicImuOut;
    std::string topicLidarIn;
    std::string topicLidarOut;
    std::string outputDir;
    int waitPeriod = -1;

    PreprocessorMetacam(const std::shared_ptr<ros::NodeHandle> &nh, bool compressed = false)
    {
        std::string packagePath = ros::package::getPath(packageName);
        nh->param<std::string>("output_dir", outputDir, packagePath + "/result/");
        CommonTools::IfNotExistThenCreate(outputDir);
        bag = std::make_shared<rosbag::Bag>(outputDir + "/data.bag", rosbag::bagmode::Write);
        int num_camera;
        nh->param<int>("max_cameras", num_camera, 2);
        cameraBagSavers.reserve(num_camera);
        std::vector<std::string> topicCameraIn;
        for(size_t i=0; i<num_camera; i++) {
            std::vector<double> distortionCoeffs;
            std::vector<double> intrinsics;
            std::vector<int> resolution;
            std::vector<int> outResolution;
            std::vector<double> vectorBuf;

            std::string topicCameraIn;
            std::string topicCameraOut;
            std::string camId = std::to_string(i);
            nh->param<std::string>("cam" + camId + "/rostopic", topicCameraIn, "");
            nh->param<std::string>("cam" + camId + "/rostopic_out_undistort", topicCameraOut, "");
            nh->param<std::vector<double>>("cam" + camId + "/distortion_coeffs", distortionCoeffs, std::vector<double>());
            nh->param<std::vector<double>>("cam" + camId + "/intrinsics", intrinsics, std::vector<double>());
            nh->param<std::vector<int>>("cam" + camId + "/resolution", resolution, std::vector<int>());
            nh->param<std::vector<int>>("cam" + camId + "/out_resolution", outResolution, std::vector<int>());
            nh->param<std::vector<double>>("cam" + camId + "/T_lidar_cam", vectorBuf, std::vector<double>());

            ROS_INFO("cam%lu/distortionCoeffs: %s", i, distortionCoeffs.size() > 1 ? (std::to_string(distortionCoeffs[0]) + " " + std::to_string(distortionCoeffs[1]) + " " + std::to_string(distortionCoeffs[2]) + " " + std::to_string(distortionCoeffs[3])).c_str() : "None");
            ROS_INFO("cam%lu/intrinsics: %s", i, intrinsics.size() > 1 ? (std::to_string(intrinsics[0]) + " " + std::to_string(intrinsics[1]) + " " + std::to_string(intrinsics[2]) + " " + std::to_string(intrinsics[3])).c_str() : "None");
            ROS_INFO("cam%lu/resolution: %s", i, resolution.size() > 1 ? (std::to_string(resolution[0]) + " " + std::to_string(resolution[1])).c_str() : "None");
            ROS_INFO("cam%lu/out_resolution: %s", i, outResolution.size() > 1 ? (std::to_string(outResolution[0]) + " " + std::to_string(outResolution[1])).c_str() : "None");
            ROS_INFO("cam%lu/T_lidar_cam: ", i);

            if (distortionCoeffs.size() != 4 || intrinsics.size() != 4 || resolution.size() != 2 || outResolution.size() != 2) {
                ROS_ERROR("The number of distortionCoeffs, intrinsics, resolution and outResolution must be 4, 4, 2 and 2 respectively!");
            }

            CameraParam camParam(std::to_string(i + 1),
                                 cv::Size(resolution[0], resolution[1]),
                                 cv::Size(outResolution[0], outResolution[1]),
                                 (cv::Mat_<double>(3, 3) << intrinsics[0], 0.0, intrinsics[2], 0.0, intrinsics[1], intrinsics[3], 0.0, 0.0, 1.0),
                                 (cv::Mat_<double>(1, 4) << distortionCoeffs[0], distortionCoeffs[1], distortionCoeffs[2], distortionCoeffs[3]));
            cameraBagSavers.emplace_back(nh, bag, camParam, std::make_pair(topicCameraIn, topicCameraOut), outputDir, compressed);
        }
    }

    ~PreprocessorMetacam()
    {
        bag->close();
    }

    void Run()
    {
        ros::Rate rate(100);
        while (ros::ok())
        {
            rate.sleep();
            ros::spinOnce();
            for (auto &cameraBagSaver : cameraBagSavers) {
                if(cameraBagSaver.Received())
                    waitPeriod = 0;
                cameraBagSaver.ResetReceived();
            }

            if (waitPeriod >= 0 && ++waitPeriod > 1000) {
                ROS_WARN("No messages received for 1000 cycles, shutting down.");
                return;
            }
        }
    }
};

int main(int argc, char* argv[]) {
    ros::init(argc, argv, "adapter_metacam");
    std::shared_ptr<ros::NodeHandle> nh = std::make_shared<ros::NodeHandle>("~");

    PreprocessorMetacam preprocessorMetacam(nh, true);

    preprocessorMetacam.Run();
    return 0;
}